#include <SPI.h>
#include "Wire.h"

#define MPU9250_ADDRESS 0x68
#define MAG_ADDRESS 0x0C

#define GYRO_FULL_SCALE_250_DPS 0x00
#define GYRO_FULL_SCALE_500_DPS 0x08
#define GYRO_FULL_SCALE_1000_DPS 0x10
#define GYRO_FULL_SCALE_2000_DPS 0x18

#define ACC_FULL_SCALE_2_G 0x00
#define ACC_FULL_SCALE_4_G 0x08
#define ACC_FULL_SCALE_8_G 0x10
#define ACC_FULL_SCALE_16_G 0x18

#define CE_PIN    8
#define MOSI_PIN  11
#define MISO_PIN  12
#define SCK_PIN   13
#define CSN_PIN   10
#define FALSE       0
#define TRUE        1
#define RX_DR       0x40
#define TX_DS       0x20
#define MAX_TR      0x10
#define MAX_RT      0x10

////////////////////////////////////////////////////////////////////////////////
//                    以下为声明部分，不建议修�?                             //
////////////////////////////////////////////////////////////////////////////////
#define         TRANS_ENHANCE_MODE    1                                         //传输类型增强�?#define         TRANS_BURST_MODE      2                                         //传输类型普通型

#define         RF13dBm                         0x3F                            // 13dBm
#define         RF10dBm                         0X0F                            // 10dBm
#define         RF8dBm                          0x15                            // 8dbm
#define         RF7dBm                          0x07                            // 7dbm
#define         RF5dBm                          0x2c                            // 5dbm
#define         RF4dBm                          0x06                            // 4dbm
#define         RF2dBm                          0x05                            // 2dbm
#define         RF0dBm                          0X0B                            // 0dBm
#define         RF_3dBm                         0x04                            // -3dBm
#define         RF_6dBm                         0x0A                            // -6dBm
#define         RF_10dBm                        0x02                            // -10dBm
#define         RF_18dBm                        0x01                            // -18dBm
#define         RF_30dBm                        0x00                            // -30dBm


/********************SPI  REGISTER  ********************/
#define   R_REGISTER      0x00                            //SPI read RF data
#define   W_REGISTER      0x20                            //SPI write RF data
#define   R_RX_PAYLOAD    0x61                            //Read RX Payload
#define   W_TX_PAYLOAD    0xA0                            //Write TX Payload
#define   FLUSH_TX      0xE1                            //Flush RX FIFO
#define   FLUSH_RX      0xE2                            //Flush TX FIFO
#define   REUSE_TX_PL     0xE3                            //Reuse TX Payload
#define   ACTIVATE      0x50                            //ACTIVATE
#define   DEACTIVATE      0x50                            //DEACTIVATE
#define   R_RX_PL_WID     0x60                            //Read width of RX data
#define   W_ACK_PAYLOAD   0xA8                            //Data with ACK
#define   W_TX_PAYLOAD_NOACK  0xB0                            //TX Payload no ACK Request
#define   CE_FSPI_ON          0xFD                            // CE HIGH
#define   CE_FSPI_OFF         0xFC                            // CE LOW
#define   RST_FSPI          0x53                            // RESET
#define   NOP_N       0xFF

/******************CONTROL  REGISTER*******************/
#define   CONFIG              0x00
#define   EN_AA       0x01
#define   EN_RXADDR     0x02
#define   SETUP_AW      0x03
#define   SETUP_RETR      0x04
#define   RF_CH       0x05
#define   RF_SETUP      0x06
#define   RF_STATUS     0x07
#define   OBSERVE_TX      0x08
#define   RPD             0x09
#define   RX_ADDR_P0      0x0A
#define   RX_ADDR_P1      0x0B
#define   RX_ADDR_P2      0x0C
#define   RX_ADDR_P3      0x0D
#define   RX_ADDR_P4      0x0E
#define   RX_ADDR_P5      0x0F
#define   TX_ADDR       0x10
#define   RX_PW_P0      0x11
#define   RX_PW_P1      0x12
#define   RX_PW_P2      0x13
#define   RX_PW_P3      0x14
#define   RX_PW_P4      0x15
#define   RX_PW_P5      0x16
#define   FIFO_STATUS     0x17
#define   DEM_CAL       0x19
#define     RF_CAL2       0x1A
#define     DEM_CAL2      0x1B
#define   DYNPD       0x1C
#define   FEATURE       0x1D
#define   RF_CAL        0x1E
#define   BB_CAL        0x1F
//-----------------------------------------------------------
#define   ADD_WIDTH     3
#define   RF_PACKET_SIZE    26      //24
#define     TRANSMIT_TYPE                 TRANS_BURST_MODE    //使用增强型模�?          TRANS_BURST_MODE    TRANS_ENHANCE_MODE
#define     DR_1M                         0X00        //通信速率 1Mbps
#define     DR_2M                         0X40        //通信速率 2Mbps
#define     DR_250K                       0XC0        //通信速率 2Mbps
#define     C_DEF_PWR                     (DR_1M|RF5dBm)
//---------------------------------------------------
//----------------------------------------------------
const byte  BB_cal_data[]    = {0x0A, 0x6D, 0x67, 0x9C, 0x46};
const byte  RF_cal_data[]    = {0xF6, 0x37, 0x5D};
const byte  RF_cal2_data[]   = {0x45, 0x21, 0xef, 0x2C, 0x5A, 0x40};
const byte  Dem_cal_data[]   = {0x01};                  // 0F for 250K, 03 for 1M&2M
const byte  Dem_cal2_data[]  = {0x0b, 0xDF, 0x02};

// const byte TR_ADDRESS[] = {0x55, 0x55, 0x55, 0x55, 0x55};
const byte TR_ADDRESS[] = {0xAA, 0xAA, 0xAA/*, 0xAA, 0xAA*/};
const byte CHANNEL_TBL[] = {04, 69, 19, 54};
byte RfRTxBuf[RF_PACKET_SIZE];
/*************************
  // declaration
**************************/
/*************************
  // Function
**************************/
/*/=============================================================
  byte SPI_RW(byte R_REG)
  {
  byte e;
  //SCK = 0;
  digitalWrite(SCK_PIN, LOW);
  //e = e;
  for (e = 0; e < 8; e++)
  {
    if (R_REG & 0x80)
      //MOSI = 1;
      digitalWrite(MOSI_PIN, HIGH);
    else
      //MOSI = 0;
      digitalWrite(MOSI_PIN, LOW);
    R_REG = R_REG << 1;
    //    SCK = 1;
    digitalWrite(SCK_PIN, HIGH);

    //byte x =  & 0xff;
    R_REG = R_REG | digitalRead(MISO_PIN);
    //    SCK = 0;
    digitalWrite(SCK_PIN, LOW);
  }
  return (R_REG);
  }
*/
unsigned char SPI_RW( unsigned char data)
{
    unsigned char i;
    for (i = 0; i < 8; i++) // 循环8�?
    {
        digitalWrite(MOSI_PIN, (data & 0x80)) ;  // byte最高位输出到MOSI
        data <<= 1;       // 低一位移位到最高位
        digitalWrite(SCK_PIN, HIGH); //SCK = 1;          // 拉高SCK，nRF24L01从MOSI读入1位数据，同时从MISO输出1位数
    data |= digitalRead(MISO_PIN);//MISO;   // 读MISO到byte最低位
    digitalWrite(SCK_PIN, LOW); //SCK = 0;          // SCK置低
    }
    return (data);        // 返回读出的一字节
}

/********************************
  RF_WriteReg:
  Writer data to RF regitster

  Input:
    address, RF register address
    wdata, Data will write to RF register
  Output:
    None
  Return:
    None
*********************************/
void RF_WriteReg( unsigned char address,  unsigned char wdata)
{
    //    CSN = LOW;
    digitalWrite(CSN_PIN, LOW);
    SPI.transfer(address);
    SPI.transfer(wdata);
    //    CSN = HIGH;
    digitalWrite(CSN_PIN, HIGH);
}
/********************************
  ucRF_ReadReg:
  Read data from RF regitster

  Input:
    address, RF register address
  Output:
    None
  Return:
    None
*********************************/
byte ucRF_ReadReg( unsigned char address)
{
    register byte tmp;

    //    CSN = LOW;
    digitalWrite(CSN_PIN, LOW);

    SPI.transfer(address);
    tmp = SPI.transfer(0);
    //    CSN = HIGH;
    digitalWrite(CSN_PIN, HIGH);
    return tmp;
}
/********************************
  RF_WriteBuf:
  Writer bust data to RF

  Input:
    T_ADDR, RF register address
    W_BUF, Data buffer
    T_WIDTH, Data size
  Output:
    None
  Return:
    None
*********************************/
void RF_WriteBuf(byte T_ADDR, byte W_BUF[], byte T_WIDTH)
{
    byte j, Temp;
    //  CSN = 0;
    digitalWrite(CSN_PIN, LOW);

    SPI.transfer(T_ADDR);
    for (j = 0; j < T_WIDTH; j++)
    {
        Temp = W_BUF[j];
        SPI.transfer(Temp);
    }
    //  CSN = 1;
    digitalWrite(CSN_PIN, HIGH);

}
/********************************
  RfInitial:
  Initial RF module

  Input:
    None
  Output:
    None
  Return:
    None
*********************************/
//================================================================================
void SpiInit(void)
{
    while (1)
    {
      digitalWrite(CSN_PIN, HIGH);    //SPI内部逻辑复位
    SPI.begin();              // SPI通讯初始化配置
    /*
        for (int i = 8; i < 14; ++i)
        {
            pinMode(i, OUTPUT);
        }
        pinMode(MISO_PIN, INPUT_PULLUP);
        digitalWrite(MOSI_PIN, LOW);
        digitalWrite(CSN_PIN, HIGH);
        digitalWrite(SCK_PIN, LOW);
        digitalWrite(CE_PIN, LOW);
  */
        delayMicroseconds(200);
        RF_WriteReg(W_REGISTER + RF_CH, 0x2A); //25 2A
        delayMicroseconds(200);
        Serial.println(ucRF_ReadReg(R_REGISTER + RF_CH), HEX);
        if (ucRF_ReadReg(R_REGISTER + RF_CH) == 0x2A) //05 2A
        {
            //    P3_7 = 0;               // CE low
            Serial.print("RF_CH:");
            Serial.println(ucRF_ReadReg(R_REGISTER + RF_CH), HEX);
            digitalWrite(CE_PIN, LOW);
            Serial.println(ucRF_ReadReg(R_REGISTER + RF_CH), HEX);
            break;
        }
        Serial.println("TRY");

    }
}

void RfInitial(void)
{
    //delay(100);                            // Delay after POWER on to wait for RF stable

    SpiInit();
    delay(100);                            // Delay after POWER on to wait for RF stable
    RF_WriteReg(RST_FSPI, 0x5A);                            //Software Reset
    delayMicroseconds(200);
    Serial.print("TEST");
    // Serial.println(ucRF_ReadReg(RST_FSPI),HEX);
    RF_WriteReg(RST_FSPI, 0XA5);
    delayMicroseconds(200);
    //Serial.println("Stack hrer");
    //while(1);
    RF_WriteBuf(W_REGISTER + BB_CAL,    BB_cal_data,  sizeof(BB_cal_data));
    delayMicroseconds(200);
    RF_WriteBuf(W_REGISTER + RF_CAL2,   RF_cal2_data, sizeof(RF_cal2_data));
    delayMicroseconds(200);
    RF_WriteBuf(W_REGISTER + DEM_CAL,   Dem_cal_data, sizeof(Dem_cal_data));
    delayMicroseconds(200);
    RF_WriteBuf(W_REGISTER + RF_CAL,    RF_cal_data,  sizeof(RF_cal_data));
    delayMicroseconds(200);
    RF_WriteBuf(W_REGISTER + DEM_CAL2,  Dem_cal2_data, sizeof(Dem_cal2_data));
    delayMicroseconds(200);


    RF_WriteReg(W_REGISTER + EN_AA,     0x01);              // Enable Auto.Ack:Pipe0
    delayMicroseconds(200);
    RF_WriteReg(W_REGISTER + EN_RXADDR, 0x01);              // Enable Pipe0
    delayMicroseconds(200);
    RF_WriteReg(W_REGISTER + SETUP_AW,  0x01);              // address witdth is 4 bytes
    delayMicroseconds(200);
    RF_WriteReg(W_REGISTER + SETUP_RETR, 0x03);             // 3 retrans...
    delayMicroseconds(200);
    RF_WriteReg(W_REGISTER + RF_SETUP, C_DEF_PWR);          // Setting the TX power
    delayMicroseconds(200);
    RF_WriteReg(W_REGISTER + RX_PW_P0,  RF_PACKET_SIZE);    // 24 bytes
    delayMicroseconds(200);
    // RF_WriteReg(W_REGISTER + RF_CH, 9);                   // RF channel=79
    // delayMicroseconds(200);
    // byte check = ucRF_ReadReg(R_REGISTER + RF_CH);
    // delayMicroseconds(200);
    // // byte checkCfg = ucRF_ReadReg(W_REGISTER + CONFIG);
    // Serial.print("Check:");
    // Serial.println(check,HEX);
    // Serial.print("checkCfg:");
    // Serial.println(checkCfg,HEX);

    //RF_WriteBuf(W_REGISTER + RX_ADDR_P0, TR_ADDRESS, ADD_WIDTH); //??è?μ??·
    delayMicroseconds(200);
    RF_WriteBuf(W_REGISTER + TX_ADDR, TR_ADDRESS, ADD_WIDTH);    //??è?μ??·


    /////////////////////////////////////////////////////////////////
    //RxMode();
    TxMode();
    // while(1);//delay();
}
void RfStateClr(void)
{
    RF_WriteReg(FLUSH_TX, 0);
    RF_WriteReg(FLUSH_RX, 0);
    RF_WriteReg(W_REGISTER + RF_STATUS, 0x70);
    RF_WriteReg(W_REGISTER + CONFIG, 0x8F);
}

/********************************
  RfSetAddr:
  Set RF address

  Input:
     pAddr: RF address parameter
  Output:
    None
  Return:
    None
*********************************/
void RfSetAddr(char *pAddr)
{
    RF_WriteBuf(W_REGISTER + RX_ADDR_P0, pAddr, ADD_WIDTH);
    RF_WriteBuf(W_REGISTER + TX_ADDR, pAddr, ADD_WIDTH);
}
/********************************
  RfSetAddr:
  Set RF default address

  Input:
    None
  Output:
    None
  Return:
    None
*********************************/
void RfSetDefaultAddr(void)
{
    char TempBuf[] = {0xAA, 0xBB, 0xCC, 0xDD};
    RfSetAddr(TempBuf);
}
/********************************
  SetRfChannel:
  Set RF channel

  Input:
    Channel: RF transceive channel
  Output:
    None
  Return:
    None
*********************************/
void SetRfChannel(byte Channel)
{
    RF_WriteReg(W_REGISTER + RF_CH, Channel);
    delayMicroseconds(200);
}
/********************************
  RxMode:
  Set receiving mode

  Input:
    None
  Output:
    None
  Return:
    None
*********************************/
void RxMode(void)
{
    //  CE = 0;
    digitalWrite(CE_PIN, LOW);
    delayMicroseconds(200);
    RF_WriteReg(FLUSH_TX, 0);
    RF_WriteReg(FLUSH_RX, 0);
    delayMicroseconds(200);
    RF_WriteReg(W_REGISTER + RF_STATUS, 0x70);
    delayMicroseconds(200);
    RF_WriteReg(W_REGISTER + CONFIG, 0x8F);
    delayMicroseconds(200);
    // byte check = ucRF_ReadReg(W_REGISTER + RF_STATUS);
    // delayMicroseconds(200);
    // byte checkCfg = ucRF_ReadReg(W_REGISTER + CONFIG);
    // Serial.print("Check:");
    // Serial.println(check,HEX);
    // Serial.print("checkCfg:");
    // Serial.println(checkCfg,HEX);
    delay(4);//4ms
    //  CE = 1;
    digitalWrite(CE_PIN, HIGH);

}
/********************************
  TxMode:
  Set transmitting mode

  Input:
    None
  Output:
    None
  Return:
    None
*********************************/
void TxMode(void)
{
    //  CE = 0;
    digitalWrite(CE_PIN, LOW);
    RF_WriteReg(W_REGISTER + CONFIG, 0x8E);
    SPI.transfer(FLUSH_TX); //清发送缓�?
    SPI.transfer(FLUSH_RX);//清接收缓�?

    RF_WriteReg(W_REGISTER + RF_STATUS, 0x70);
    digitalWrite(CE_PIN, HIGH);
    delayMicroseconds(200);
}
/********************************
  SetRfPwr:
  Set RF transmit power

  Input:
    PwrLvl, transmit power
  Output:
    None
  Return:
    None
*********************************/
//void SetRfPwr(byte PwrLvl)
//{
//  RF_WriteReg(W_REGISTER + RF_SETUP, PwrLvl|(RF_POWER&0xC0));         // |(RF_POWER&0xC0): 除了码率参数外，功率参数全部�?
//}
/********************************
  ReceiveRfData:
  Receive RF data from RX buffer.

  Input:
     pDataBuf, address to be stored data from RF fifo
  Output:
    Data from RF fifo
  Return:
    result of receiving this data frame
*********************************/
byte ReceiveRfData(byte *pDataBuf)
{
    byte TR_status, i;
    TR_status = ucRF_ReadReg(RF_STATUS);                    // 读取RF状�?  // Serial.println(TR_status&0x70);
    if ((TR_status & 0x70) == 0x40)                     // 判断是否收到数据
    {
        //    CE = 0;                                 // CE引脚拉低，退出接收以免在读取数据时又收到新的数据
        //    CSN = 0;
        // Serial.println("find");
        digitalWrite(CE_PIN, LOW);
        digitalWrite(CSN_PIN, LOW);


        SPI.transfer(R_RX_PAYLOAD);                         // 读取RF数据
        for (i = 0; i < ( RF_PACKET_SIZE); i ++)
        {
            *(pDataBuf + i) = SPI.transfer(0);
        }
        //    CSN = 1
        digitalWrite(CSN_PIN, HIGH);

        RF_WriteReg(FLUSH_RX, 0);                       // 清除接收缓存
        RF_WriteReg(FLUSH_TX, 0);                       // 清除发射缓存
        RF_WriteReg(W_REGISTER + RF_STATUS, 0x70);      // 重置RF状�?    //    CE = 1;                                 // CE引脚拉高，继续接�?    digitalWrite(CE_PIN, HIGH);

        return TRUE;
    }
    return FALSE;
}
/********************************
  SendRfFrame:
  Sending RF data in TX buffer.

  Input:
    None
  Output:
    None
  Return:
    RF sending frame state
    Bit4 Max retry
    Bit5 Sending complete
    Bit7 RF module failed
*********************************/
byte SendRfFrame(byte *TxBuf)
{
    unsigned char i;
    char TrState;

    RF_WriteBuf(W_TX_PAYLOAD, TxBuf, RF_PACKET_SIZE);               // Write data to be transmit to RF FIFO
    //  CE = 1;                                     // Start transmitting
    digitalWrite(CE_PIN, HIGH);

    delayMicroseconds(100);                                 // CE 保持高电平约100uS，如果太长或者太短都可能导致丢码
    //  CE = 0;
    digitalWrite(CE_PIN, LOW);

    for (i = 0; i < 255; i++)
    {
        // Wait until transmitting complete
        delayMicroseconds(1);
        TrState = ucRF_ReadReg(R_REGISTER + RF_STATUS) & 0x70;        // 读取RF发射状�?    if (TrState)
        {
            break;                                // 若发射结束，跳出等待
        }
    }
    RF_WriteReg(FLUSH_TX, 0);
    RF_WriteReg(FLUSH_RX, 0);
    RF_WriteReg(W_REGISTER + RF_STATUS, 0x70);
    return TrState;
}
/********************************
  //载波测试
*********************************/
//void RfCarrierTest(void)
//{
//  const byte Dem_cal_data_Carrier[1] = {0xE1};
////  CE = 0;
//  digitalWrite(CE_PIN,LOW);
//
//  delay(50);  //delay 50ms
//  RF_WriteReg(W_REGISTER + CONFIG, 0x0E);
//    RF_WriteReg(W_REGISTER + RF_CH, 0x28);            //单载波频�?//    RF_WriteReg(W_REGISTER + RF_SETUP, RF_POWER);           //13dbm
//    RF_WriteBuf(W_REGISTER + DEM_CAL,   Dem_cal_data_Carrier, sizeof(Dem_cal_data_Carrier));
//  delay(50);
//}

///////////////////////////////////////////////////////////////////
byte gPid;
byte F_SendUart = 0;
byte  R_Lost_Tx1, R_Lost_Tx2;
byte F_LinkSlave1;//   = UserFlag1^0;
byte F_DisUpDate;//    = UserFlag1^1;
byte F_Match;//            = UserFlag1^2;
byte F_LinkSlave2;//   = UserFlag1^3;
byte F_DisUpDate2;//       = UserFlag1^4;
byte F_EnUpSend;//     = UserFlag1^5;
byte F_MatchOk1;//         = UserFlag1^6;
byte F_MatchOk2;//         = UserFlag1^7;

byte gRfBuffer[RF_PACKET_SIZE];
byte R_USER_FLAG;
byte R_USER_FLAG_VALUE;
byte F_TMR_BASE;
#define C_MAX_IDLE_TMR          (500/33)
#define CONFI                   0x00

byte R_T_BASE_CNT;
byte gIdleTmr;


byte F_User_ID1;//   = UserFlag2^0;
byte F_User_ID2;//   = UserFlag2^1;
//byte F_SendUart;//   = UserFlag2^2;
byte F_OneTime;//    = UserFlag2^3;
byte F_ReadSafety;// = UserFlag2^4;
byte F_SendQM;//     = UserFlag2^5;
byte RfLostChnCnt;
byte Data_64[24] = {};
byte  R_CNT_24, R_CNT2_24;
byte  RfChnInx;
const byte  CHANNEL_TBL0[] = {9, 42, 77};
void SetNextChn(void)
{
    RfLostChnCnt = 300;
    RfChnInx ++;
    if (RfChnInx >= 3)
    {
        RfChnInx = 0;
    }
    SetRfChannel(CHANNEL_TBL0[RfChnInx]);
}
void ResetIdleTmr(void)
{
    gIdleTmr = 0;
}
void PacketRfFrame(void)
{
//  gRfBuffer[0] = gRfBuffer[0]+1;
//  gRfBuffer[1] = 0xff;
//  gRfBuffer[2] = 0xff;
//  gRfBuffer[3] = 0xff;
  
}
void WriteRfFifo(byte RfAdress, byte Len)
{
    byte x, rTemp;

    //IO_RF_CSN = 0;
    digitalWrite(CSN_PIN, LOW);
    SPI.transfer(RfAdress);
    for (x=0; x<Len; x++)
    {
        SPI.transfer(gRfBuffer[x]);
    }
    //IO_RF_CSN = 1;
    digitalWrite(CSN_PIN, HIGH);
}

byte SendRfData(void)
{
    byte y=10;
    byte RetVaule;
    WriteRfFifo(W_TX_PAYLOAD,RF_PACKET_SIZE);
    delayMicroseconds(250); //for 1M/2M
    //DelayMs(2); //for 250Kbps
    while(y--)
    {
        delayMicroseconds(250);
        RetVaule = ucRF_ReadReg(R_REGISTER+RF_STATUS);
        if((RetVaule&(MAX_RT|TX_DS)) != 0)
        {
            break;
        }
    }
    RF_WriteReg(FLUSH_TX,0);
    RF_WriteReg(FLUSH_RX,0);
    RF_WriteReg(W_REGISTER + RF_STATUS,0X70);
    return RetVaule;
}
byte CheckIdleTmrout(byte KeyState)
{
    if(KeyState==0)
    {
        gIdleTmr++;
        if(gIdleTmr>C_MAX_IDLE_TMR)
        {
            return true;
        }
        else
        {
            return false;
        }
    }
    else
    {
        gIdleTmr = 0;
        return false;
    }
}
void RfSleep(void)
{
    //IO_RF_CE = 0;
    digitalWrite(CE_PIN, LOW);
    RF_WriteReg(W_REGISTER + CONFI, 0);                                         // д��RF˯������
    //TRISC &= (~B_MISO);
    digitalWrite(MISO_PIN, LOW);
    //IO_RF_MISO = 0;                                                               // ����MISO���0����ֹRF��������©�硣
    //IO_RF_CE = 0;
    digitalWrite(CE_PIN, LOW);
}

long current;
void setup()
{
    // put your setup code here, to run once:
    mpu9250_init();
    Serial.begin(38400);
    delay(10);
    RfInitial();
    delay(40);

}

void loop()
{
   // Read accelerometer and gyroscope
  uint8_t Buf[14];
  I2Cread(MPU9250_ADDRESS, 0x3B, 14, Buf);

  // Create 16 bits values from 8 bits data

  // Accelerometer
  int16_t ax = (Buf[0] << 8 | Buf[1]);
  int16_t ay = (Buf[2] << 8 | Buf[3]);
  int16_t az = (Buf[4] << 8 | Buf[5]);

  // Gyroscope
  int16_t gx = (Buf[8] << 8 | Buf[9]);
  int16_t gy = (Buf[10] << 8 | Buf[11]);
  int16_t gz = (Buf[12] << 8 | Buf[13]);
  // Read magnetometer data
  I2CwriteByte(MAG_ADDRESS, 0x37, 0x02);
  uint8_t Mag[7];
  I2Cread(MAG_ADDRESS, 0x03, 7, Mag);
  byte button = digitalRead(13);

  byte x = map(analogRead(A2),0,1023,0,255);
  byte y = map(analogRead(A3),0,1023,0,255);
  if (Mag[0] == 0xff && Mag[1] == 0xff && Mag[2] == 0xff)
  {
    mpu9250_init();
  }
  // Magnetometer
  int16_t mx = (Mag[3] << 8 | Mag[2]);
  int16_t my = (Mag[1] << 8 | Mag[0]);
  int16_t mz = (Mag[5] << 8 | Mag[4]);
  int16_t time_send = millis()&0xffff;
  byte check_sum = 0;
  byte pac[26] = {0xf0,0x55,0x18,Buf[0],Buf[1],Buf[2],Buf[3],Buf[4],Buf[5],
                  Buf[8],Buf[9],Buf[10],Buf[11],Buf[12],Buf[13],Mag[3],
                  Mag[2],Mag[1],Mag[0],Mag[5],Mag[4],time_send>>8,
                  time_send&0xff,button,(x&0xf0)|(y&0x0f),check_sum};
  for (int i = 0; i < 25; ++i)//计算前25个数据的总和
  {
    check_sum = check_sum+pac[i];
  }
  pac[25] = check_sum;//将总和跟新到第26个数据上
//  if (millis()- t_pre >= 10)
//  {
//    t_pre = millis();
//    Serial.write(pac,26);//发送26个数据包
//  }
    byte rTemp;
    if(millis()-current>8)
    {
        current = millis();
        //F_TMR_BASE = false;
        //PacketRfFrame();
        memcpy(gRfBuffer,pac,26);
    SetRfChannel(9);
        SendRfData();
    SetRfChannel(42);
        SendRfData();
    SetRfChannel(77);
        SendRfData();
    }
    //if (!RfLostChnCnt&&millis()-current>=40)
    if (!RfLostChnCnt)
    {
        // Serial.print("Run here:");

        // Serial.println(millis()-current);
        //SetNextChn();
        //current = millis();
    }
    else
    {
        RfLostChnCnt--;
    }
}
void mpu9250_init()
{
  Wire.begin();
  // Configure gyroscope range
  I2CwriteByte(MPU9250_ADDRESS, 27, GYRO_FULL_SCALE_2000_DPS);
  // Configure accelerometers range
  I2CwriteByte(MPU9250_ADDRESS, 28, ACC_FULL_SCALE_16_G);
  // Set by pass mode for the magnetometers
  I2CwriteByte(MPU9250_ADDRESS, 0x37, 0x02);

  // Request first magnetometer single measurement
  I2CwriteByte(MAG_ADDRESS, 0x0A, 0x16);
}

// This function read Nbytes bytes from I2C device at address Address.
// Put read bytes starting at register Register in the Data array.
void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t *Data)
{
  // Set register address
  Wire.beginTransmission(Address);
  Wire.write(Register);
  Wire.endTransmission();

  // Read Nbytes
  Wire.requestFrom(Address, Nbytes);
  uint8_t index = 0;
  while (Wire.available())
    Data[index++] = Wire.read();
}

void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data)
{
  // Set register address
  Wire.beginTransmission(Address);
  Wire.write(Register);
  Wire.write(Data);
  Wire.endTransmission();
}


